package com.hitqz.robot.driver.zmq.protocol.handler;

import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import java.math.BigDecimal;
import java.math.RoundingMode;
import java.util.*;

/**
 * 激光实时数据处理类
 *
 * @author wengyx
 * @date 2021-09-14 11:17
 **/
@Slf4j
@Component
public class LaserScanHandler implements HandlerZMQService {
    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.LaserScan laserScan = AddressBookProtos.LaserScan.parseFrom(data);
        String type = laserScan.getDescriptorForType().getName();
        int objId = laserScan.getHeader().getObj();
        int ackId = laserScan.getHeader().getAck();
        // 激光实时数据 主动上报
        if (objId == AddressBookProtos.LaserScan.OBJ_ID.NUL_SCAN_FRAME_NTF_VALUE) {
            RobotMap map = WheelToolkit.robotMap;
            PoseDto currentPose = (PoseDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE);
            if (map!=null) {
                // 2D和PB地图需要转换坐标
                if (!Objects.equals(map.getType(),1) && !Objects.equals(map.getType(),6)){
                    return;
                }
                List<PoseDto> scanList = getRobotPostLaserscan(currentPose, laserScan, map);
                WheelCacheUtil.put(WheelCacheUtil.KEY_LASER_SCAN, scanList);
            }
        }
        //释放锁
        WheelToolkit.release(ackId,type, objId, true);

    }


    /**
     * 2D激光数据转换为坐标（web显示坐标）
     * 圆点坐标：(x0,y0) 
     * 半径：r 
     * 角度：a0 
     * 则圆上任一点为：（x1,y1） 
     * x1   =   x0   +   r   *   cos(ao   *   3.14   /180 ) 
     * y1   =   y0   +   r   *   sin(ao   *   3.14   /180 )
     *
     * @return
     */
    private static List<PoseDto> getRobotPostLaserscan(PoseDto robotPoint, AddressBookProtos.LaserScan lscan, RobotMap map) {
        List<PoseDto> pointFList = new ArrayList<>();
        float angleMax = lscan.getAngleMax();
        float angleMin = lscan.getAngleMin();
        float rangeMin = lscan.getRangeMin();
        float rangeMax = lscan.getRangeMax();
        float angleIncrement = lscan.getAngleIncrement();
        int inc = 0;
        for (float range : lscan.getRangesList()) {
            if (range == Float.POSITIVE_INFINITY || range == Float.NEGATIVE_INFINITY) {
                inc++;
                continue;
            }
            if (range < rangeMin || range > rangeMax) {
                inc++;
                continue;
            }
            float yaw = BigDecimal.valueOf(robotPoint.getYaw() ).setScale(3, RoundingMode.HALF_UP).floatValue();//机器人姿态
            float xb = (float) (range * Math.cos(angleMin + angleIncrement * inc));//机器人坐标系的表示
            float yb = (float) (range * Math.sin(angleMin + angleIncrement * inc));//机器人坐标系的表示
//            float x1 = (float) (robotPoint.getX() + (rangeMin + range) * Math.cos(radian2Angle(yaw + angleMin + angleIncrement * inc) * Math.PI / 180));
            //range不是相对值
            float x1 = (float) (robotPoint.getX() +  Math.cos(yaw) * xb - Math.sin(yaw) * yb) ;//地图坐标系下的表示
//            float y1 = (float) (robotPoint.getY() + (rangeMin + range) * Math.sin(radian2Angle(yaw + angleMin + angleIncrement * inc) * Math.PI / 180));
            float y1 = (float) (robotPoint.getY() +  Math.sin(yaw) * xb + Math.cos(yaw) * yb);//地图坐标系下的表示
            //todo MapTheta
//            double theta = Optional.ofNullable(map.getMapTheta()).orElse(0.0f);//原点
            double theta = 0.0f;
            double dx = x1 - map.getMapOriginx();//原点
            double dy = y1 - map.getMapOriginy();//原点
            double cosTheta = Math.cos(theta);
            double sinTheta = Math.sin(theta);
            double resolution = map.getMapResolution();
            float x = BigDecimal.valueOf((dx * cosTheta + dy * sinTheta) / resolution).setScale(3, RoundingMode.HALF_UP).floatValue();
            //这边正负根据前端那边显示去调整,导航那边是-(dx * sinTheta - dy * cosTheta),web和导航y轴相反
            float y = BigDecimal.valueOf((dx * sinTheta - dy * cosTheta) / resolution).setScale(3, RoundingMode.HALF_UP).floatValue();
            PoseDto point = new PoseDto(x, y, 0f, (yaw - (float)theta + angleIncrement));
            pointFList.add(point);
            inc++;
        }
        return pointFList;
    }

    public static double radian2Angle(float radian) {
        double tmp = Math.round(radian / Math.PI * 180);
        return tmp > 0 ? tmp : 360 + tmp;
    }

}
